
#include "Transformation.h"

#include "g2o/core/graph_optimizer_sparse.h"
#include "g2o/core/hyper_graph.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/math_groups/se3quat.h"
#include "g2o/core/structure_only_solver.h"
#include "g2o/types/slam3d/vertex_se3_quat.h"
#include "g2o/types/slam3d/edge_se3_quat.h"

#include <string>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/passthrough.h>

#include <Eigen/Core>
#include <fstream>
/*
using namespace std;

void Transformation::print()
{
	printf("src:%i <--> dst:%i\n",src->id,dst->id);
	printf("weight: %f\n",weight);
	printf("Level:  %i\n",level);
	printf("   __                                          __\n");
	printf("  |                                              |\n");
	for(int i = 0; i < 4; i++)
	{
		printf("  |  ");
		for(int j = 0; j < 4; j++)
		{
			if(transformationMatrix(i,j) >= 0){printf(" ");}
			printf("%5.5f   ",transformationMatrix(i,j));
		}
		printf("|\n");
	}
	printf("  |__                                          __|\n");
	transformationMatrix;
}

void Transformation::display()
{
	IplImage* img_a = cvLoadImage( src->frame->rgb_path.c_str(), 1);
	IplImage* img_b = cvLoadImage( dst->frame->rgb_path.c_str(), 1);
	
	int sx = img_a->width;
	int sy = img_a->height;
		
	IplImage* img_combine = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
	IplImage* img_combine_clone = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
	for (int i = 0; i < sx; i++)
	{
		for (int j = 0; j < sy; j++)
		{
			img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 0] = img_a->imageData[3 * (j * sx + i) + 0];
			img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 1] = img_a->imageData[3 * (j * sx + i) + 1];
			img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 2] = img_a->imageData[3 * (j * sx + i) + 2];
		}
	}
	for (int i = 0; i < sx; i++)
	{
		for (int j = 0; j < sy; j++)
		{
			img_combine->imageData[3 * (j * (2*sx) + (i)) + 0] = img_b->imageData[3 * (j * sx + i) + 0];
			img_combine->imageData[3 * (j * (2*sx) + (i)) + 1] = img_b->imageData[3 * (j * sx + i) + 1];
			img_combine->imageData[3 * (j * (2*sx) + (i)) + 2] = img_b->imageData[3 * (j * sx + i) + 2];
		}
	}
	
	for(int i = 0; i < src->frame->keypoints->nr_valid_points;i++)
	{
		cvCircle(img_combine,cvPoint(src->frame->keypoints->valid_key_points[i]->frame_x+sx, src->frame->keypoints->valid_key_points[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
	}
	for(int i = 0; i < dst->frame->keypoints->nr_valid_points;i++)
	{
		cvCircle(img_combine,cvPoint(dst->frame->keypoints->valid_key_points[i]->frame_x, dst->frame->keypoints->valid_key_points[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
	}
	for(int i = 0; i < nr_matches ;i++)
	{
		//cvCopy(img_combine,img_combine_clone,NULL);
		//cvCircle(img_combine_clone,cvPoint(matches_src[i]->frame_x+sx,matches_src[i]->frame_y), 5,cvScalar(0, 255, 0, 0),1, 8, 0);
		//cvCircle(img_combine_clone,cvPoint(matches_dst[i]->frame_x+sx,matches_src[i]->frame_y ), 5,cvScalar(0, 255, 0, 0),1, 8, 0);
		//cvLine(img_combine,cvPoint(matches_src[i]->frame_x+sx,matches_src[i]->frame_y),cvPoint(matches_dst[i]->frame_x+0,matches_src[i]->frame_y ),cvScalar(0, 255, 0, 0),1, 8, 0);
		//printf("%f,%f <---> %f,%f\n",matches_src[i]->frame_x,matches_src[i]->frame_y,matches_dst[i]->frame_x,matches_dst[i]->frame_y);
	}
	cvShowImage("Transformation image", img_combine);
	cvReleaseImage( &img_combine);
	cvReleaseImage( &img_combine_clone);
	cvReleaseImage( &img_a);
	cvReleaseImage( &img_b);
	cvWaitKey(0);
}

void Transformation::displayDifference(Transformation * transformation)
{
	if(src == transformation->src && dst == transformation->dst)
	{
		printf("Can do comparison\n");
		IplImage* img_a = cvLoadImage( src->frame->rgb_path.c_str(), 1);
		IplImage* img_b = cvLoadImage( dst->frame->rgb_path.c_str(), 1);
	
		int sx = img_a->width;
		int sy = img_a->height;
		
		IplImage* img_combine = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 0] = img_a->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 1] = img_a->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 2] = img_a->imageData[3 * (j * sx + i) + 2];
			}
		}
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 0] = img_b->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 1] = img_b->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 2] = img_b->imageData[3 * (j * sx + i) + 2];
			}
		}
	
		for(int i = 0; i < src->frame->keypoints->nr_valid_points;i++)
		{
			cvCircle(img_combine,cvPoint(src->frame->keypoints->valid_key_points[i]->frame_x, src->frame->keypoints->valid_key_points[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
		}
		for(int i = 0; i < dst->frame->keypoints->nr_valid_points;i++)
		{
			cvCircle(img_combine,cvPoint(dst->frame->keypoints->valid_key_points[i]->frame_x + sx, dst->frame->keypoints->valid_key_points[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
		}
		printf("nr_matches org: %i\n",nr_matches);
		printf("nr_matches new: %i\n",transformation->nr_matches);
		for(int i = 0; i < nr_matches ;i++)
		{
			KeyPoint * src_keypoint = matches_src[i];
			KeyPoint * dst_keypoint = matches_dst[i];
			bool found = false;
			for(int j = 0; j < transformation->nr_matches ;j++)
			{
				if(src_keypoint == transformation->matches_src[j] && dst_keypoint == transformation->matches_dst[j])
				{
					//cvLine(img_combine,cvPoint(src_keypoint->frame_x,src_keypoint->frame_y),cvPoint(dst_keypoint->frame_x+sx,dst_keypoint->frame_y ),cvScalar(0, 255, 0, 0),1, 8, 0);
					found = true;
					break;
				}
			}
			if(!found)
			{
				cvLine(img_combine,cvPoint(src_keypoint->frame_x,src_keypoint->frame_y),cvPoint(dst_keypoint->frame_x+sx,dst_keypoint->frame_y ),cvScalar(255, 0, 0, 0),1, 8, 0);
				//printf("no match blue: %f : %f <--> %f : %f\n",src_keypoint->frame_x,src_keypoint->frame_y,dst_keypoint->frame_x,dst_keypoint->frame_y);
			}
		}
		
		for(int i = 0; i < transformation->nr_matches;i++)
		{
			KeyPoint * src_keypoint = transformation->matches_src[i];
			KeyPoint * dst_keypoint = transformation->matches_dst[i];
			bool found = false;
			for(int j = 0; j < nr_matches ;j++)
			{
				if(src_keypoint == matches_src[j] && dst_keypoint == matches_dst[j])
				{
					//printf("%i,%i\n",i,j);
					found = true;
					break;
				}
			}
			if(!found)
			{
				//printf("no match purp: %f : %f <--> %f : %f\n",src_keypoint->frame_x,src_keypoint->frame_y,dst_keypoint->frame_x,dst_keypoint->frame_y);
				cvLine(img_combine,cvPoint(src_keypoint->frame_x,src_keypoint->frame_y),cvPoint(dst_keypoint->frame_x+sx,dst_keypoint->frame_y ),cvScalar(255, 0, 255, 0),1, 8, 0);
				//printf("didnt find for %i\n",i);
			}
		}
		cvShowImage("Transformation comparison image", img_combine);
		cvReleaseImage( &img_combine);
		cvReleaseImage( &img_a);
		cvReleaseImage( &img_b);
	}
}

void Transformation::load(string path,FrameNode ** nodes)
{
	printf("Transformation load file: %s\n",path.c_str());
	printf("works\n");
	ifstream infile (path.c_str(),ifstream::binary);
	infile.seekg(0,ifstream::end);
	long size=infile.tellg();
	char * buffer_char 		= new char [size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float	= (float *) buffer_char;
	double * buffer_double	= (double *) buffer_char;
	infile.seekg(0);
	infile.read (buffer_char,size);
	infile.close();
	
	int counter = 0;
	int src_id 	= buffer_int[counter++];
	int dst_id 	= buffer_int[counter++];
	src = nodes[src_id];
	dst = nodes[dst_id];
	nr_matches 	= buffer_int[counter++];
	level 		= buffer_int[counter++];
	weight 		= buffer_double[counter];
	counter += 2;
	transformationMatrix = Eigen::Matrix4f::Identity();
	transformationMatrix(0,0) = buffer_float[counter++];
	transformationMatrix(0,1) = buffer_float[counter++];
	transformationMatrix(0,2) = buffer_float[counter++];
	transformationMatrix(0,3) = buffer_float[counter++];
	
	transformationMatrix(1,0) = buffer_float[counter++];
	transformationMatrix(1,1) = buffer_float[counter++];
	transformationMatrix(1,2) = buffer_float[counter++];
	transformationMatrix(1,3) = buffer_float[counter++];
	
	transformationMatrix(2,0) = buffer_float[counter++];
	transformationMatrix(2,1) = buffer_float[counter++];
	transformationMatrix(2,2) = buffer_float[counter++];
	transformationMatrix(2,3) = buffer_float[counter++];
	
	transformationMatrix(3,0) = buffer_float[counter++];
	transformationMatrix(3,1) = buffer_float[counter++];
	transformationMatrix(3,2) = buffer_float[counter++];
	transformationMatrix(3,3) = buffer_float[counter++];
	
	matches_src = new KeyPoint*[nr_matches];
	matches_dst = new KeyPoint*[nr_matches];
		
	for(int i = 0; i < nr_matches; i++)
	{
		matches_src[i] = src->frame->keypoints->valid_key_points[buffer_int[counter++]];
		matches_dst[i] = src->frame->keypoints->valid_key_points[buffer_int[counter++]];
	}
	delete buffer_char;
	//printf("src_id: %i, dst_id: %i, nr_matches: %i\n",src_id,dst_id,nr_matches);
	//exit(0);
}

void Transformation::store(string path)
{
	printf("Transformation out file: %s\n",path.c_str());
	
	long size 				= sizeof(int)*(4+2*nr_matches) + sizeof(int)*16 + sizeof(double)*1;
	char * buffer_char 		= new char[size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float 	= (float *) buffer_char;
	double * buffer_double 	= (double *) buffer_char;
	int counter = 0;
	buffer_int[counter++] = src->id;
	buffer_int[counter++] = dst->id;
	buffer_int[counter++] = nr_matches;
	buffer_int[counter++] = level;
	buffer_double[counter] = weight;
	counter += 2;
	buffer_float[counter++] 		= transformationMatrix(0,0);
	buffer_float[counter++] 		= transformationMatrix(0,1);
	buffer_float[counter++] 		= transformationMatrix(0,2);
	buffer_float[counter++] 		= transformationMatrix(0,3);
	
	buffer_float[counter++] 		= transformationMatrix(1,0);
	buffer_float[counter++] 		= transformationMatrix(1,1);
	buffer_float[counter++] 		= transformationMatrix(1,2);
	buffer_float[counter++] 		= transformationMatrix(1,3);
	
	buffer_float[counter++]		 	= transformationMatrix(2,0);
	buffer_float[counter++]		 	= transformationMatrix(2,1);
	buffer_float[counter++]		 	= transformationMatrix(2,2);
	buffer_float[counter++]		 	= transformationMatrix(2,3);
	
	buffer_float[counter++]		 	= transformationMatrix(3,0);
	buffer_float[counter++]		 	= transformationMatrix(3,1);
	buffer_float[counter++]		 	= transformationMatrix(3,2);
	buffer_float[counter++]		 	= transformationMatrix(3,3);
	
	for(int i = 0; i < nr_matches; i++)
	{
		printf("%i, %i\n",matches_src[i]->index_number,matches_dst[i]->index_number);
		buffer_int[counter++] = matches_src[i]->index_number;
		buffer_int[counter++] = matches_dst[i]->index_number;
	}
	ofstream outfile (path.c_str(),ofstream::binary);
	outfile.write (buffer_char,size);
	outfile.close();
	
//	printf("have to store the actual matches...\n");
}
*/
